import asyncio
import cv2
import base64
import json
import threading
import time

msg = {
    "topic": "/ubiquityrobot/camera_node/image_raw/compressed",
    "msg": {
        "header": {
            "stamp": {
                "secs": 1641971401,
                "nsecs": 217771053
            },
            "frame_id": "",
            "seq": 3
        },
        "data": "", 
        "format": "jpeg"
    },
    "op": "publish"
}

response = {
    "op": "service_response",
    "service": "/ubiquityrobot/camera_node/camera_set_enable",
    "values": {
        "port": 0,
        "value": 0
    },
    "result": True,
    "id": "call_service:/ubiquityrobot/camera_node/camera_set_enable:ID"
}

active = False

class Usb_Camera:
    def __init__(self):
        self.ws = None

        pass

    def register(self, func_map):
        func_map['/ubiquityrobot/camera_node/camera_set_enable'] = self.camera_set_enable

    async def camera_set_enable(self, ws, args, id):
        global active
        websocket = ws
        self.ws = ws
        response['values'] = args
        response['id'] = f"call_service:/ubiquityrobot/camera_node/camera_set_enable:{id}"
        await websocket.send(json.dumps(response))
        if args['value'] == 1 and active is False:
            print('open camera')
            active = True
            # 启动永久后台任务
            self.bg_task = asyncio.create_task(self.startCapture())

        elif args['value'] == 0 and active is True:
            print('close camera')
            active = False
            self.bg_task.cancel()
            try:
                await self.bg_task
            except asyncio.CancelledError:
                print("程序退出")

    async def startCapture(self):
        cap = cv2.VideoCapture(0)
        if not cap.isOpened():
            print("Cannot open camera")
            return
        while active:
            # 逐帧捕获
            ret, frame = cap.read()
            # 如果正确读取帧，ret为True
            if not ret:
                print("Can't receive frame (stream end?). Exiting ...")
                break
            if frame is not None:
                # print("frame captured")
                retval, buffer = cv2.imencode('.jpg', frame)
                data = base64.b64encode(buffer)
                msg["msg"]["data"] = data.decode("utf-8")
                # print(msg)
                await self.ws.send(json.dumps(msg))
            await asyncio.sleep(0.06)
        cap.release()
        cv2.destroyAllWindows()